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[57] ABSTRACT 

This invention is a robot control system based on a high 
level language implementing a spatial operator algebra. 
There are two high level languages included within the 
system. At the highest level, applications programs can be 
written in a robot-oriented applications language including 
broad operators such as MOVE and GRASP. The robot- 
oriented applications language statements are translated into 
statements in the spatial operator algebra language. Pro- 
gramming can also take place using the spatial operator 
algebra language. The statements in the spatial operator 
algebra language from either source are then translated into 
machine language statements for execution by a digital 
control computer. The system also includes the capability of 
executing the control code sequences in a simulation mode 
before actual execution to assure proper action at execution 
time. The robot’s environment is checked as part of the 
process and dynamic reconfiguration is also possible. The 
languages and system allow the programming and control of 
multiple arms and the use of inward/outward spatial recur- 
sions in which every computational step can be related to a 
transformation from one point in the mechanical robot to 
another point to name two major advantages. 


20 Claims, 20 Drawing Sheets 
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HIGH LEVEL LANGUAGE-BASED ROBOTIC 
CONTROL SYSTEM 

This is a division, of application Ser. No. 07/786,499, 
filed Nov. 1, 1991 now U.S. Pat. No. 5,303,384, which is a 5 
continuation-in-part of Ser. No. 07/459,438, filed Jan. 2, 
1990 now abandoned. 

ORIGIN OF THE INVENTION 

10 

The invention described herein was made in the perfor- 
mance of work under a NASA contract, and is subject to the 
provisions of Public Law 96-517 (35 USC 202) in which the 
Contractor has elected not to retain title. 

1. Technical Field 15 

The invention relates to computer systems for controlling 

robots and, more particularly, to a high level language-based 
control system for a robot comprising, a digital control 
computer operably connected to the robot to control the 
movement functions thereof, the control computer including 20 
program execution means for executing control programs; 
means for inputting the control programs into the digital 
control computer for execution thereby; and, translator 
means for reading sequences of robot control statements and 
for generating control programs therefrom defining robot 25 
control sequences, the translator means comprising, first 
translator means for accepting as inputs thereto statements in 
a high level spatial operator algebra language and for 
outputting machine language sequences reflecting the inputs 
to the first translator means and executable by the digital 30 
control computer, and second translator means for accepting 
as inputs thereto statements in a high level robot applications 
language and for outputting statements in the high level 
spatial operator algebra language reflecting the inputs to the 
second translator means. 35 

The preferred embodiment additionally comprises robot 
simulation means operably connected to the digital control 
computer for executing the programs in a simulation mode 
prior to executing them to control the robot and for allowing 
the digital control computer to execute the programs to 40 
control the robot only if no problems exist in the simulation 
mode. It also includes reconfiguration means connected to 
the robot simulation means for reconfiguring the programs 
when problems exist in the simulation mode to a form in 
which no problems exist in the simulation mode before 45 
allowing the digital control computer to execute the pro- 
grams to control the robot. 

Also in the preferred embodiment, robot and environmen- 
tal definition means are connected to the robot simulation 5Q 
means for holding and providing data about the robot and the 
environment in which the robot is working which is used to 
determine if problems exist in the simulation mode. Prefer- 
ably, the robot and environmental definition means includes 
means for dynamically changing the data about the robot 55 
and the environment in which the robot is working. 

2. Background Art 

Robotic control systems and robots are a fairly new 
technology. The first digital computer- controlled apparatus 
operating in real time began to appear about the early 1960s. 60 
Before that, “control” took the form of analog controllers 
which could read analog inputs from sensors (e.g. pressure, 
temperature, etc.) and output electrical, pneumatic, or other 
forms of control signals to mechanical operators connected 
thereto for opening valves, etc. whereby the controlled 65 
function could be maintained at pre-established operating 
levels. 
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With the advent of digital computers, a number of sensors 
and operators could be connected to the computer and be 
controlled by the programs thereof. The programs, of course, 
could implement far more elaborate control logic for the 
system as a whole than the proportional band, rate, and reset 
functions typically implemented by the prior art controllers 
operating on an individual (and non-interconnected) basis. 

What could probably be considered as the first industrial 
“robots” were the numerically controlled (NC) machines 
and assembly apparatus. Where previously such apparatus 
was operated under the control of human operators (with 
attendant possibility of error), the NC-controlled machines 
could repetitively perform manufacturing and assembly 
operations without error. Early versions of NC machines 
employed pre-punched paper tape inputs to provide the 
positional movement sequences for the machine. Later, the 
numerical sequences were stored in the memory of a digital 
computer. 

While the mechanical aspects of such industrial (where 
the term “industrial” includes robots employed in space 
operations) robots have become more exotic in the period 
since their first introduction, the control (and programming) 
considerations thereof have not moved much beyond the 
starting point. Take for example the robot 10 of FIG. 1. The 
robot 10 comprises an arm 12 attached to a base 14 on one 
end and having a gripping hand 16 on the opposite end. The 
arm 12 comprises an upper portion 18 and a lower portion 
20. There is a powered “shoulder” joint 22 connecting the 
upper portion 18 to the base 14, a powered “elbow” joint 24 
connecting the upper portion 18 to the lower portion 20, and 
a “wrist” joint 26 connecting the lower portion 20 to the 
gripping hand 16. The robot 10 is connected to have the 
powered joints 22, 24, 26 operated by a computer 28 so as 
to effect desired movement of the gripping hand 16 as, for 
example, to pick up and move an object. While early NC 
machines were simply moved known amounts from a known 
starting point to effect their functions, robots such as robot 
10 typically have force and motion sensors (not shown) 
incorporated within their joints so that there is feedback 
information available to the computer 28. Also, with an 
articulated arm such as arm 12, the dynamics of movement 
are much more exotic and important to the accuracy of the 
robot than simply moving a clamped part in X and Y axes 
along a bed under a rotating machine tool which is moved 
up and down in the Z axis to cause the machining of the part. 
Accordingly, the instructional sequences to the joints 22, 24, 
and 26 from the computer 28 are highly complex; and, like 
the NC apparatus where the sequence of movements to effect 
the desired machining of a part was pre-programmed into 
the controlling apparatus, the sequence of movements for 
the joints 22, 24, and 26 is pre-programmed into the com- 
puter 28. If any changes occur in the robot 10 or in the 
actions to be performed thereby, the instruction sequence 
must be re-programmed. 

To fully comprehend the problem at hand, one must 
consider the typical environment for digital computer pro- 
gramming and the prior art aids provided therefor. The 
computer 28 only performs slavishly a sequence of machine 
language instructions which have been pre-loaded for execu- 
tion at run time. In the execution of its instructions, a 
real-time computer can perform arithmetic calculations, 
make decisions, input dynamic information from sensors, 
and input information from tables. The tables can contain 
pre-established information and/or dynamically altered 
information. Typically, as depicted in FIG. 5, there are 
pre-defined instruction run-time support programs 40 (such 
as subroutines) that can be used by the main instruction 
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sequence to accomplish routine tasks of a repetitive nature 
(such as trigonometric function calculation) necessary to the 
accomplishment of overall system performance. While the 
first programmers were forced to do their programming in 
machine language (an extremely tedious and error-prone 5 
process), support programs in the form of assemblers and 
then compilers (such as the compiler 30 of FIG. 5) made 
later (and present) efforts much easier to program and to 
debug. Through processes of experimentation and trial and 
error, formulas for expressing movement of the various arms 1Q 
and joints of a robot have been established to some degree 
of accuracy. While these formulas are complex and include 
many terms, the programming process has been greatly 
simplified by high level language compilers such as FOR- 
TRAN. The system’s analyst/programmer must still estab- 15 
lish and set down the sequence of formulas which is 
assumed will accomplish the desired sequence of move- 
ments to effect a required end result. As depicted in FIG. 2, 
the sequence of formulas comprising the movement instruc- 
tion sequence is then input to a compiler 30 (a program 2Q 
running in a computer) which outputs a sequence of machine 
language instructions which are then loaded into the robot 
controlling computer 28 for later execution. 

The instruction sequence to move the hand 16 of the robot 
10 so as to pick up the object 32 in FIG. 1 can run several 25 
pages. Note that this is with the base 14 fixed and the object 
32 fixed at a pre-established location. The instruction 
sequence is of such length simply to account for all the 
forces and inertial factors of the various joints 22, 24, 26 and 
the arm portions 18, 20 as the gripping hand 16 is moved 30 
from an initial point, to the object 32, and from thence to a 
final point. The object, of course, is to move the hand 16 
smoothly along a trajectory from point to point and not in a 
jerky fashion constantly correcting from deviation errors. 
The robot must also apply a pre-described amount of force 35 
or stress to the objects that it is handling. As in all real-time 
movement control systems, a principle design factor in the 
system design and programming process is the elimination 
of any instability in the feedback control loop. Instability can 
lead to the robot 10 going into increased oscillations (lead- 40 
ing finally to an error shutdown before damage can occur) or 
the inability to finally position the hand 16 at a desired point 
because of excessive overshoot as the result of each move- 
ment in a corrective direction. 

Because of the foregoing approach to basic robotic pro- 45 
gramming and control, a computer-controlled two armed 
robot as shown in FIG. 3 is virtually impossible employing 
prior art techniques. This is particularly true in situations 
where the robot task is not numerically pre-programmed and 
must be changed or designed by the human operator at run 50 
time. The object of the robot 10' shown in FIG. 3 is for the 
two arms 12, 12' to pick up the two object halves 34, 34’, 
bring them into alignment with one another, and push them 
together to form the object 32 while simultaneously lifting 
the object 32 up to and placing it on the shelf 33. A human 55 
performing the task would first pick up the two object halves 
34, 34', bring them into alignment, and then push them 
together to form the object 32. Having formed the object 32, 
the object 32 would then be lifted up and be placed on the 
shelf 33. Obviously, the robot 10’ has the potential to 60 
perform the assembly and placement process simulta- 
neously. Using the prior art programming technique 
described above, however, it would be virtually impossible 
for a human programmer to program the sequence of opera- 
tions necessary to cause the robot 10' to do so. As can be 65 
appreciated from pondering the various aspects which must 
be taken into consideration as compared with the simple 
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one-arm robot of FIG. 1, the complexity with two arms and 
two components to perform a combined assembly and 
placement operation is vastly greater. 

Another aspect which is not even considered in prior art 
robotic control schemes is changing dynamics of the envi- 
ronment. Take the situation depicted in FIG. 4, for example. 
This is the single armed robot 10 of FIG. 1 into which an 
obstacle 36 has been placed. If the robot 10 has been 
pre-programmed to move the hand 16 along the trajectory 
indicated by the dashed arrow 38, it will do so and strike the 
obstacle 36 rather than moving to the alternate trajectory 
indicated by the dashed arrow 40 as necessary to reach over 
the obstacle 36. 

Yet another aspect of robotic control which is not even 
considered by prior art robotic control systems is the check- 
ing of anticipated movement in a fast simulation mode prior 
to actual implementation on a real-time basis to assure that 
what is desired has a high probability of occurring and 
adjusting the projected movement until the desired object is 
achieved in simulation prior to actual implementation. 

STATEMENT OF THE INVENTION 

Accordingly, it is an object of this invention to provide a 
programming and run-time support package for robotic 
sy stem configuring and operation which is simple and easy 
to use. 

It is another object of this invention to provide a pro- 
gramming and run-time support package for robotic system 
configuring and operation which permits the programmer/ 
system’s analyst to direct the operation of complex robotic 
equipment which cannot be programmed by prior art tech- 
niques. 

It is still another object of this invention to provide a 
programming and run-time support package for robotic 
system configuring and operation which checks anticipated 
movement in a simulation mode prior to actual implemen- 
tation on a real-time basis to assure that what is desired has 
a high probability of occurring and which adjusts the pro- 
jected movement until the desired object is achieved in 
simulation prior to actual implementation. 

It is a further object of this invention to provide a 
programming and run-time support package for robotic 
system configuring and operation which checks a dynami- 
cally -changeable environment definition to assure capability 
of intended movement and redefine trajectories as necessary 
to assure non-interference with intended movement. 

It is a still further object of this invention to provide a 
means to easily program and modify the mathematical 
models that must be embedded into a robot control computer 
in order for it to exhibit a certain amount of intelligence in 
responding to higher level user commands such as MOVE, 
GRASP, etc. 

A further object of this invention is to provide a very 
efficient implementation of the above-mentioned math- 
ematical models by means of a very efficient set of algo- 
rithms (i.e. sequences of computational steps) that compute 
the necessary robot trajectories and motions. 

Another object of this invention is to provide a very 
efficient set of algorithms which implement all robot math- 
ematical computations recursively and in which the number 
of arithmetical operations (additions, multiplications, divi- 
sions, etc.) increasing only linearly with the number of 
degrees of freedom. 

Other objects and benefits of this invention will become 
apparent from the detailed description which follows here- 
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inafter when taken in conjunction with the drawing figures 
which accompany it. 

BRIEF DESCRIPTION OFF THE DRAWINGS 

5 

FIG. 1 is a simplified drawing of one-arm robot system as 
wherein the present could be used. 

FIG. 2 is a block diagram of a prior art high-level 
compilation approach to generating instructions for a com- 
puter controlling a robot. 10 

FIG. 3 is a simplified drawing of two-arm robot system as 
wherein the present provides significant benefits. 

FIG. 4 is the simplified drawing of one-arm robot system 
of FIG. 1 showing a dynamic situation which is unsol vable 
by prior art robotic control systems. 15 

FIG. 5 is a block diagram of a prior art high-level 
compilation and run-time approach to computers utilized in 
real-time control systems such as robotics. 

FIG. 6 is a simplified diagram showing the multi-level 2 o 
approach to a robotic control system and the generation of 
instructions therefor. 

FIG. 7 is a functional block diagram of a robotic instruc- 
tion generation and run-time system according to the present 
invention in a preferred embodiment thereof. 25 

FIG. 8 is a drawing giving several examples of robotic 
functions stated in the spatial operator algebra of the present 
invention. 

FIG. 9 is a drawing depicting how the various operators 
of a robotic functions stated in the spatial operator algebra 30 
of the present invention are also defined in terms of the 
spatial operator algebra. 

FIG. 10 is a block diagram of a robotic function stated in 
the spatial operator algebra of the present invention. 

FIG. 11 is a drawing depicting in another manner how the 
various operators of a robotic functions stated in the spatial 
operator algebra of the present invention are also defined in 
terms of the spatial operator algebra and how this definition 
process exists on many levels. 

FIG. 12 is a functional block diagram showing how two 
applications language functions such as SQUEEZE and 
MOVE can be implemented using the spatial operator 
algebra of the present invention. 

FIG. 13 is a functional diagram depicting how the capa- 
bility of the present invention to allow inward recursion 
permits alternate approaches to solving robotics problems 
not possible with the prior art approaches. 

FIG. 14 is the block diagram of a robotic function stated 
in the spatial operator algebra of the present invention of 
FIG. 10 showing how the approach of the present invention 
allows portions to be easily removed so as to dynamically 
reconfigure the problem being solved or obtain necessary 
information. 

FIG. 15 is a graph of number of arithmetic operations 
versus number of degrees of freedom depicting how the 
recursive approach of this invention reduces the number of 
arithmetic operations even for moderately complex prob- 
lems. 

FIG. 16 is a graph of number of symbolic terms for 
computed torque control methods depicting how the spatial 
operator approach allows order or magnitude reductions in 
the number of terms used by a system designer. 

FIG. 17 is a block diagram of a robot embodying the 
invention and illustrating the modular hierarchy of high 
level and low level operators. 
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FIGS. 18 through 24 are block diagrams of respective 
high level operators each modularly assembled in seven 
levels of low level operators. 

FIG. 25 illustrates a table of high level operators which 
can be constructed in accordance with the invention. 

FIG. 26 is a block diagram of a modular high level 
operator for linearized dynamic robotic models. 

DETAILED DESCRIPTION OF THE 
INVENTION 

The construction of the robotic program generation and 
run-time support system 42 of the present invention is 
depicted symbolically in FIG. 6. Unlike the prior art real- 
time compiler and run-time system of FIG. 5, the system 42 
of this invention is comprised of multiple nested layers of 
increased complexity. The programmer/analyst deals only 
with the outer simplified layers while the complex inner 
layers are transparent. Moreover, as will be seen shortly, the 
outermost layer is very simple and intended for use by 
unsophisticated applications programmers while the next 
layer is more complex and intended for programmers with 
training in system modification and enhancement. 

The system 42 of the present invention is made possible 
by the inclusion of two high level language implementations 
within one compilation system. This is novel in and of itself 
to the best of the applicant’s knowledge. Typically, a com- 
piler accepts input statements in its high level language 
syntax. The final output is a string of machine language 
instructions (the object program) which will perform the 
logic of the high level language statement sequence as input 
(i.e. the high level language program). Internally, the com- 
piler may break the high level language statements into 
intermediate language (IL) statements which are then turned 
into object code by various “passes” of the compiler. The IL 
statements are never seen or accessed by a user of the 
compiler and are only part of a compiler implementation 
scheme which allows a compiler to generate object code (i.e. 
machine language) for different computers by the replace- 
ment of only a small portion of the programming which 
translates the IL into machine language. 

By contrast, the system 42 of this invention accepts 
applications level high level statements from an applications 
programmer into the outer layer 44. As will be discussed in 
greater detail shortly, the applications programmer describes 
control and trajectory objectives in broad terms such as 
MOVE, GRASP, etc. The output of the outer layer 44 is a 
series of spatial operator algebra statements which comprise 
the second high level language of the system. Programming 
can also be done in the spatial operator algebra statements by 
those more skilled in the programming and robotic system 
analysis art so as to easily and accurately define the various 
robotic functions. The spatial operator algebra statements 
(either as output by the outer layer 44 or as directly input 
thereto) are compiled by the second layer 46, which outputs 
statements of implementation formulae such as those used in 
the prior art for initial programming. Thus, it can be seen that 
the programming job is simplified by either one or two 
layers of programming language simplification in the system 
42 of the present invention. The second layer 46 output is 
input to the third layer 48, which performs the spatial 
recursions with support from the utility routines of the inner 
layer 50. 

The system 42 of the present invention can be seen from 
a different perspective by reference to FIG. 7. An applica- 
tions programmer wishing to program a sequence of opera- 
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tions for the two-armed robot 10 ’ of FIG. 3 would do so in 
a series of high-level statements oriented to robot movement 
and control. As indicated in the figure, representative opera- 
tors might include MOVE, SQUEEZE, GRASP, and the 
like. For example, if the grasping hands 16 of the arms 12 5 
of the robot 10 ' were designated as A and B, a programmer 
might include a statement sequence such as: 

MOVE A TO LOCATION 1 GRASP OBJECT 
MOVE A TO LOCATION 2 RELEASE 

As can be seen, the programmer works with simplified, 1Q 
straight forward statements and concepts. Unlike the prior 
art approach to robotic programming as described above, the 
esoteric aspects of robotic movement are completely trans- 
parent on the applications level. The robot programming 
language of this invention differs from other existing lan- 
guages in the following areas: 

* It coordinates the motion of two or more arms working 
in concert to execute a task. 

* It has embedding therein a complete mathematical 
model of the robot which allows precise control of the 
forces applied to objects — this model being transparent 
to the user. 

* Motion and force models contained therein are also 
completely transparent to the user. 

* It contains algorithms which execute very fast because 25 
they implement all robot mathematical computations 
recursively. 

The applications language is input to a first level compiler 
52 as depicted in FIG. 7. The compiler 52 interprets the 
syntax and content of the applications statements in the 30 
manner of any typical compiler and outputs a sequence of 
spatial operator algebra statements which, if executed with 
proper support and supplemental run-time inputs, will per- 
form the applications functions called for by the applications 
statement sequence which was input. Such compilation 35 
processes and related matters are, of course, well known to 
those skilled in the art and, therefore, in the interest of 
simplicity and the avoidance of redundancy such matters 
will not be addressed in any detail herein. 

The system 42 of the invention employs a spatial operator 40 
algebra. This algebra is described in detail in a paper by the 
one of the inventors herein, Guillermo Rodriguez, “Random 
Field Estimation Approach to Robot Dynamics” IEEE 
Transactions on Systems, Man and Cybernetics , Vol. 20, No. 

5, September/October 1990, pp. 1081-1093 and in other 45 
publications cited below herein. The algebra is described 
below herein with reference to cited definitions of low level 
operators given in the pulications referred to herein. The 
spatial operator algebra described herein is a specialized 
high level computer language. As in other specialized com- 50 
puter languages, there are operators referring to operations 
unique to the robotic control and manipulation environment. 

For example, the FORTRAN language is intended for the 
implementing of formulas comprised principally of arith- 
metic and trigonometric functions. Thus, FORTRAN 55 
employs operators such as +, — , *, and / to indicate add, 
subtract, multiply, and divide, respectively. JOVIAL, on the 
other hand, is intended for logic and record manipulation. 
Thus, in addition to the arithmetic operators it includes 
operators such as IF, GREATER, EQUAL, and THEN. 

Complex functions included within the complement of 
operators of a high level language are typically supported by 
run-time programs implemented as subroutines. Thus, a * 
(multiply) operator generates a MULTIPLY command in 
machine language as this is an operation typically supported 65 
by the hardware of a digital computer. On the other hand, a 
TAN(f) [i.e. tangent of f] operation will generate a return 
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branch to the tangent subroutine passing the “f ’ value in the 
process. In this regard, the spatial operator algebra of the 
present invention is no exception and no novelty is claimed 
for such an implementation. Thus, the various operators of 
the spatial operator algebra, when compiled, generate return 
branches to various subroutines which accomplish uniquely 
robotic control and manipulation functions; and, they pass 
the associated parameters necessary to the particular com- 
putation to the subroutine. 

As those skilled in the art will readily appreciate, by 
making both the applications operators and spatial operator 
algebra operators usable by programmers, the system 42 of 
this invention becomes a very powerful tool for robotic 
applications. As will undoubtedly be surmised by those 
15 skilled in the art from the from the foregoing brief descrip- 
tion, the applications operators are implemented by the first 
level compiler 52 basically as macros. In other words, when 
the compiler 52 sees an applications operator, it substitutes 
its equivalent sequence of spatial operator algebra operators. 
20 This is not typical of a high level language compiler; but, 
provides the system 42 of this invention with a reconfig- 
urability not typical of other high level language systems. 
Usually, a compiler has a fixed programming sequence 
which generates the machine language statements which 
implement the logic and formulas of the high level language 
input stream. By employing a macro -based approach, 
trained users (i.e. systems programmers) can reconfigure 
their compiler to generate spatial operator algebra operator 
sequences that more accurately implement the intent of the 
applications operators for the specific robotic environment 
of interest. 

This reconfigurability is a multi-level capability within 
the system 42 of this invention. Spatial operator algebra 
operator sequences generated by programmers or the first 
level compiler 52 are input to a second level compiler 54. 
The second level compiler 54 is also macro-based and 
supported by run-time subroutines. Thus, while it is more 
difficult and requires a highly skilled individual to accom- 
plish it, both the second level compiler 54 and the support 
subroutines can be modified, if necessary, to more accurately 
perform the robotic functions in a particular environment. 
The support subroutines which actually perform the func- 
tions of the spatial operator algebra operators must actually 
implement the highly complex instructional sequences as 
described above which have been used in the prior art to 
cause the robotic elements to perform as desired and 
required. Within the system 42 of this invention, however, 
these complex sequences are broken down to their simplest 
elements which are, in turn, implemented as subroutines. 
Thus, to reconfigure the way in which a particular spatial 
operator algebra operator is implemented, the subroutine 
calling sequences comprising the support subroutine per- 
forming that operator and/or the subroutines themselves 
need only be modified. 

The foregoing is an important aspect of the present 
invention which is unique to the present invention and, 
therefore, deserves further explanation, description, and 
example. For this purpose, we will leave FIG. 7 for the 
moment and turn first to FIG. 8. As depicted therein each 
60 typical robotic function is defined in terms of spatial opera- 
tor algebra operators. As depicted in FIG. 9, each spatial 
operator algebra operator has a built-in order-N implemen- 
tation algorithm which is accomplished by a series of 
subroutines which can also be described in terms of the 
spatial operator algebra operators. Let us assume that a 
typical single-line operator equation to be implemented is as 
follows: 



5,519,814 


9 

&(hL*)ir V2 (H>*Qr 1 b)D~ l/2 (i-L)T 

The block diagram thereof appears in FIG. 10. A different 
representation of the implementation of the single-line 
operator equation M“ 1 =(I-L*)D“ 1 (I-L) appears in FIG. 11. 
As can be seen more clearly therein, each operator is defined 
in terms of the operators which comprise it. Thus, the actual 
programming of machine language code is confined to the 
bottom level operators which can no longer be defined in 
terms of other spatial operator algebra operators. The major 
programming effort for initial operation definition and for 
subsequent redefinition (if necessary) is accomplished using 
the simple spatial operator algebra operators of this inven- 
tion. 

It is important to note at this point that the spatial operator 
algebra of this invention reduces by at least two orders of 
magnitude the number of symbols that the system designer 
has to use to specify the mathematical model embedded in 
the robot control computer. It thereby organizes computa- 
tions and computer programs in the robot control computer 
into a hierarchical modular framework that is easy to test and 
debug. The number of symbols used is minimal. Thus, there 
can never be anything simpler for implementing robot 
control mathematical models. In this regard, attention is 
directed to the two graphs of FIGS. 15 and 16. As depicted 
in FIG. 15, with the recursive approach of this invention 
employing the unique algebra thereof, the algorithms imple- 
ment all robot mathematical computations recursively such 
that the number of arithmetic operations (additions, multi- 
plications, divisions, etc.) increases only linearly with the 
number of degrees of freedom whereas prior art is based on 
algorithms wherein the number of operations increases with 
the square or cube of the number of degrees of freedom. This 
advantage is quite significant as soon as robots including six 
or more degrees of freedom are used. FIG. 16 depicts how, 
for example, when accomplishing torque control the prior 
art trigonometric approach will require over 200 symbolic 
terms, the prior art recursive approach (the most commonly 
used technique at this time) will require approximately 100 
symbolic terms, and the spatial operator algebra of this 
invention will require less than 10. 

As those skilled in the art will undoubtedly have realized 
by now, the flexible nature of the spatial operator algebra of 
this invention provides similar flexibility in the definition of 
applications operators. In this regard, see for example FIG. 
12. Recalling the operational problem postulated for FIG. 3 
(i.e. pick up, assemble, and move to shelf), if the operators 
MOVE and SQUEEZE have been defined within the system, 
a new operator MOVE AND SQUEEZE can be imple- 
mented by simply combining the two individual elements 
thereof. This is effectively accomplished by using another 
novel approach to robotic control programming only pos- 
sible with the system 42 of the present invention. The 
concept is illustrated in FIG. 13. Given that there is a robot 
comprising two articulated arms 12 having segments 56 
joined at powered joints 58 holding a task object 60. In a 
typical prior art robotic control system, the programmer is 
constrained (because of the extreme complexity of the 
problem and the limited available tools) to attack program- 
ming of the arm from an outward recursion point of view. As 
stated earlier, it would be virtually impossible to program a 
two arm cooperative effort using prior art techniques; but, 
assuming that it was possible, the above outward recursion 
limitation would apply. By contrast, the ease of use of the 
system 42 of the present invention allows the programmer to 
compute the inertia of each arm 12 as seen from the tip or 
outermost end. Thus, as depicted in FIG. 13, the arm, joint, 
and task object problem is converted into a task object 
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problem having two point inertias 62 (corresponding to the 
two arms 12) associated therewith. To move the task object 
60 from point A to point B, therefore, an inward recursions 
to effect movement of the two point inertias 62 are per- 
5 formed (using the easily implemented spatial operator alge- 
bra, of course). The necessary movement of each joint 58 
simply falls out of the calculation. 

Another benefit of the modular approach to robotic con- 
trol and manipulation of the present invention which may 
10 not be readily apparent is depicted in FIG. 14. Given the 
problem described above with respect to FIG. 13, simple 
editing, as represented by the dashed box 64, can be used to 
detach the task object 60 from the arms 12. This, of course, 
can even take place dynamically at run time. 

15 Returning once again to the overall system block diagram 
of FIG. 7, additional features of the system 42 of the present 
invention in its preferred embodiment will now be 
described. The run-time instruction sequence to be executed 
is input to the control computer 28. This is, of course, 
20 accomplished in the usual manner for object code and is 
shown symbolically as an input arrow for convenience and 
simplicity only. The computer run-time package portion of 
the system 42 of the present invention includes a simulation 
sequencer 66. Using the input run-time instruction sequence 
25 and supported by the spatial operator algebra definitions 68 
and robot & environmental definition information 70, the 
simulation sequencer 66 first performs each anticipated 
move in a simulation mode to see if the probability is good 
that the operation will produce the desired effect. If an 
30 interposed object is in a proposed trajectoiy in the manner of 
FIG. 4, a new trajectory can be constructed. This is accom- 
plished by the reconfiguration logic 72. This is a very 
important and novel- aspect of the system 42 of this inven- 
tion. The robot & environmental definition information 70 
35 contains necessary information and parameters required to 
perform the spatial operator algebra at run time. Note that 
the robot & environmental definition information 70 has 
provision for dynamic input. Thus, when a component of the 
controlled robot is changed in a manner which will effect its 
40 operation, the new information is inserted by the robot 
operator/repairman into the robot & environmental defini- 
tion information 70. On-line electronic/optical scanning 
and/or sensing apparatus can search the field of movement 
of the robot for the intrusion of interfering objects and insert 
45 such information into the robot & environmental definition 
information 70. In this manner, the controlled robot is best 
able to cope with changes in its personal and environmental 
conditions and best achieve the objectives for which is was 
installed. No prior art robotic control system has such a 
50 capability. Of course, when the simulated sequence is deter- 
mined by the simulation sequencer to achieve the desired 
objectives to a satisfactory level, the run-time instruction 
sequence in its acceptable form is then passed to the execu- 
tion sequencer 74 for actual execution and output of control 
55 signals to the controlled robot. 

WORKING EXAMPLE 

One exemplary implementation of the invention will now 
60 be described with reference to FIG. 17. A user who is 
acquainted only with the simple high level operator lan- 
guage of the invention uses a computer 66 (such as a PC or 
work station) to enter a sequence of high level language 
statements into a robot 68. This sequence unambiguously 
65 defines a desired robot system or architecture. The sequence 
of high level language statements is stored in a buffer 
memory 70. 
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In the present example, the contents of the memory 70 
corresponds to FIG. 12. The example of FIG. 12 was 
selected for tutorial purposes as one of the simplest robots 
capable of performing work on an object. The task per- 
formed by the robot of FIG. 12 is that of simultaneously 5 
moving and squeezing an object that is being handled by a 
two- arm robotic mechanism. Such an operation would occur 
for example in transporting bulky or irregularly shaped 
objects. The technical challenge is to move the object 
precisely while at the same time applying very precise force 
distributions and internal stresses on the object. Handling of 
fragile glasses is another example of a task requiring simul- 
taneous move and squeeze operations. 

The task begins with a command issued by the user, or by 
a higher-level user interface, to MOVE (72) to a desired 
position x d and to SQUEEZE (74) the object with a desired 1 
squeeze force f sd . Based upon sensor readings obtained from 
a contact force sensor 76 and a squeeze projection operator 
F s (78) corresponding to a high level language statement, a 
comparison 80 is made between the actual sensed force and 
the desired force f sd , and the difference is applied to a force 20 
controller 82 characterized by a gain g f whose output is 
summed (84) with the desired squeeze force f, d and applied 
to a sum 86 . Simultaneously, based upon sensor readings 
obtained from position sensors 88 of the angular position 0 
of each robot joint and a forward kinematics operator X (90), 25 
a comparison 92 is made between the actual position x of 
each robot joint and the desired position x d , and the differ- 
ence is applied to a position controller 94 characterized by 
a gain g x . The output of the position controller 94 is 
transformed by a move projection operator F m (96) whose 30 
output is applied to the sum 86 . The output of the sum 86 is 
transformed by an adjoint Jacobian operator J* (98) whose 
output is a vector T defining the joint moments applied to the 
robot joints. The foregoing may be summarized by the 
following high level language expression, of the type which 35 
the user would enter at the user input 66 : 

x d ]}. ( 1 ) 

The foregoing expression, defining the applied robot joint 40 
moments, is a mixture in the sense that it simultaneously 
commands and regulates the desired MOVE operations and 
the desired SQUEEZE operations. Two independent con- 
trollers 82, 94 are required, one (82) being capable of 
squeezing the object but not moving it and the other (94) 45 
being capable of moving the object but not squeezing it. 

The high level language statement in the sequence stored 
in the memory 68 is transmitted as a sequence of parameters 
(f, f sd , gf, 9, x d , g x ) and high level operators (F s , F m , X and 
J*) to a high level compiler 100 shown in FIG. 17 (corre- 50 
sponding to the first level compiler 52 of FIG. 7). The high 
level compiler 100 translates each one of the high level 
operators in the statement into the appropriate sequence of 
low level operators. As shown in FIG. 17, the high level 
compiler 100 contains low level operator sequences for each 55 
one of the high level operators F s , F m , X and J* employed in 
the system definition illustrated in FIG. 12. This feature will 
be described below in greater detail with reference to FIG.’S 
18-23. The high level operators shown inside the high level 
compiler 100 of FIG. 17 comprise a minimum set of high 60 
level operators sufficient to construct a robot capable of 
performing work on an object, and are a relatively small 
subset of a more complete set tabulated in FIG. 24. 

Continuing the description of FIG. 17, the high level 
compiler 100 transmits each sequence of low level operators 65 
to a low level compiler 102 (corresponding to the second 
level compiler 54 of FIG. 7). The low level compiler 102 
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translates each low level operator received from the high 
level compiler 100 to the appropriate sequence of real time 
execution software modules. Each real time execution soft- 
ware module corresponds to a sequence of assembly lan- 
guage arithmetic instructions. As illustrated in FIG. 17, the 
low level compiler 102 contains real time execution soft- 
ware module sequences for each one of a complete set of 
low level operators. The low level operators shown inside 
the low level compiler 102 of FIG. 17 comprise a complete 
set for implementing almost any type of robot operation and 
architecture. Significantly, all of them are required to imple- 
ment even the extremely simple robot architecture of present 
example of FIG. 12. 

The real time execution software modules in each 
sequence adduced by the low level compiler 102 for each 
low level operator are transmitted to a run time instruction 
memory 104 of a control processor 106. The processor 106 
sequentially fetches the real time execution software mod- 
ules from the memory 104 and compiles each one of the real 
time execution software modules into a corresponding 
sequence of assembly language arithmetic instructions. It 
then executes the arithmetic instructions employing the 
parameters f, f sd , g p 0 , x d , g x as operands in accordance with 
the now thrice compiled version of the original statement of 
Eqn. (1). Some of the parameters (i.e., g f and g x ) are 
constants obtained from the design specifications of the 
robot 68 and are simply stored in the memory 104 prior to 
run time. The parameters f and 0 are received by the 
processor 106 from the sensors 76, 88 on the robot 68 while 
the parameters 4, and are received by the processor 106 
as the squeeze and move commands 72 and 74. Since the 
real time execution software modules stored in the memory 
104 are directly translated by the processor 106 to arithmetic 
instructions, the terms “real time execution software mod- 
ule” and “arithmetic instruction” may be used interchange- 
ably hereinafter. 

Each of the low level operators 102 shown inside the low 
level compiler 102 of FIG. 17 has been previously defined 
in publications by the inventors herein, including Guillermo 
Rodriguez et al., “A Spatial Operator Algebra for Manipu- 
lator Modeling and Control”, International Journal of 
Robotics Research , August 1991 (referred to hereinafter as 
“publication 1”), Guillermo Rodriguez, “Recursive Forward 
Dynamics for Multiple Robot Arms Moving a Common 
Task Object”, IEEE Transactions on Robotics and Automa- 
tion , Vol. 5, No. 4, August 1989, pp. 510-521 (referred to 
hereinafter as “publication 2”), Guillermo Rodriguez, “Kal- 
man Filtering, Smoothing, and Recursive Robot Arm For- 
ward and Inverse Dynamics”, IEEE Journal of Robotics and 
Automation, Vol. RA-3, No. 6 , December 1987, pp. 624-639 
(referred to hereinafter as “publication 3”) and Guillermo 
Rodriguez, “Random Field Estimation Approach to Robot 
Dynamics,” IEEE Transactions on Systems, Man and Cyber- 
netics , Vol. 20, No. 5, pp. 1081-1093 (referred to hereinafter 
as “publication 4”). For the sake of clarity, Table I below 
provides the citations to definitions or descriptions of each 
of the low level operators of the low level compiler 102 of 
FIG. 17. The “adjoint” (denoted by a *) of each operator 
listed in Table I is found by merely taking its complex 
conjugate. 

TABLE I 

operator publication location 

A(C) 2 p. 513, eqn. 11 

B pickoff projection 1 4th page, line 2 
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TABLE I-continued 


operator 

publication 

location 

C point contact 

2 

p. 513, lines 26-40 

G Kalman gain 

1 

8th page, line 4 

G(k) 

3 

p. 630, eqn. 39 

H(k) 

1 

3rd page, line 13 

H joint axis projection 

1 

4th page, line 1 

L 

4 

p. 1088, line 34-35 

M 

1 

5th page, line 14 

M(k) 

1 

5th page, line 17 

P(k) 

3 

p. 630, eqn. 43 

<|>(k,k-i) 

1 

3rd page, line 15 

$ 

1 

4th page, line 3 

y(kk-l) 

1 

8th page, eqn. 4.2 

V 

1 

9th page, line 3 

A k coordinate transform 

3 

p 628, cl 2, In 17-27 


CONNECTING MODULAR BUILDING BLOCKS 

OF LOW LEVEL OPERATORS TO FORM HIGH 
LEVEL OPERATORS 

The foregoing low level operators form a library of 
components of which an unlimited number of copies may be 
made and connected together like modular building blocks 
in many different ways to form different high level operators 25 
that the system designer may need. For this purpose, a 
system designer input 108 may be used to name and then 
define a new high level operator to the high level operator 
compiler 100. The system designer input 108 refers to a 
menu 110 of the complete set of low level operators (cor- 30 
responding to Table I) which the system designer may select 
as building blocks to be connected together in various 
combinations to form new high level operators. The system 
designer input 108 is used to specify these connections, 
thereby defining the named high level operator whose defi- 35 
nition is then stored in the high level compiler 100. This 
feature may be used to expand the high level operator 
language of the invention. 

Each high level operator generally consists of a number of 
levels of operations, each level providing results used in 40 
carrying out the operations of the next highest level. Usually, 
there are seven levels in each high level operator. Generally, 
each low level operator always occurs, if at all, in a 
particular one of the seven levels, as follows: 

7th Level: 

The link coordinate transform operator ACG*) generally 
occurs in the beginning (seventh) level and transforms the 
coordinates to the frame of reference of a current (kth) link. 
This operation is carried out individually for each link in a 5 o 
robot member or arm. 

6th Level: 

Operators used in cross link and cross joint operations 
such as <j>(k,k-l), (j)*(k,k-l), H(k), H*(k), G(k) and G*(k) 
generally occur in the next (sixth) level and are carried out 55 
for each link individually. 

5th Level: 

Operations carried out at the sixth level provide operators 
residing at the next highest (fifth) level, such as the intra- 6Q 
body propagation operators \y(k,k-l) and \}f*(k,k-l), which 
are computed individually for all links/joints. 

4th Level: 

The fourth level generally consists of inward or outward 
recursions of the propagation operators, usually comprising 65 
a sum of a certain propagation operator over all previous 
links. 


3rd Level: 

The sum carried out in the 4th level provides one of a 
group of “global” operators, such as the Kalman transition 
operator \jr, for the next highest (third) level. This group of 
operators also includes the “global” operators such as H, G, 
D and/or B. These operators are “global” in the sense that 
they represent computations for all links k=l through N. 

2nd Level: 

The operators adduced in the 3rd level are combined in 
the second level, implementing Kalman filter processes in 
those cases in which the cross-joint operation of Level 6 is 
a true transformation and not simply an identity matrix. The 
Kalman filter operations correspond to recursions across 
plural robot links for a given discrete (constant) point in 
time. (In contrast, most prior art applications of Kalman 
filters perform recursions across many discrete points in 
time.) For those cases in which the cross-joint operation of 
Level 6 is merely an identity matrix, there is no “true” 
cross-joint operation, and the resulting recursion operation 
at Level 5 is that of a degenerate Kalman filter so that no 
Kalman prediction update is performed in Level 2. Refer- 
ence is hereby made to the desciption of the foregoing type 
of Kalman filter process described at page 630 of publication 
3 referred to in Table I. As described therein, each iteration 
of the Kalman filter provides additional components such as 
G(k) of global operators such as the Kalman gain G. 

1st Level: 

The 2nd level provides the final result of the current 
iteration of the first level. 

The foregoing seven levels of operations are carried out 
starting with a first iteration of Levels 1 through 7 at a first 
discrete point in robot operation time to produce a result at 
Level 1, then a second iteration of Levels 1 through 7 at the 
next discrete point in time using the results obtained in the 
previous iteration to produce a next result at the Level 1, and 
so forth. 

How to form each of the high level operators as modular 
combinations of low level operators in a seven level archi- 
tecture will now be described for each of the high level 
operators required in the robot of FIG. 12. 

Inverse Jacobian Operator J -1 

The modular assembly of independent low level operators 
comprising the inverse Jacobian operator J -1 is illustrated in 
FIG. 18. Referring to FIG. 18, level 7 consists of a coordi- 
nate transformation A k to the frame of reference of the 
current (kth) link. This is followed by level 6 consisting of 
an adjoint cross-link operator <j)*(k+l,k) and an adjoint 
cross-joint operation I-H*(k)G*(k), whose results are com- 
bined in the next level, level 5, to produce the adjoint 
link-to-link operator \j/*(k,k~l). Such adjoint link-to-link 
propagators obtained in all previous iterations are summed 
in the outward recursion of level 4 to produce the adjoint 
Kalman transition operator tjf* of level 3 and its complex 
conjugate y. Level 3 also comprises the pickoff operator B, 
the Kalman gain adjoint operator G* (adduced from the 
G*(k) of all previous iterations in accordance with Table 1) 
and the joint axis unit vector adjoint operator H* (adduced 
from the H*(k) of all previous iterations in accordance with 
Table I). As indicated in FIG. 18, Level 3 also comprises the 
spatial articulated inertia D, which is another high level 
operator compiled in a similar seven-level architecture and 
will be described below with reference to FIG. 21. Level 3 
further comprises the Kalman transition operator \| / which is 
derived from the outward recursion of level 4 through a 
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conventional complex conjugate operation. Level 2 consists 
of several complex operations operating on various elements 
obtained in Level 3: a spatial Kalman smoother G*\|/*H* 
generates L* in accordance with the definition of L refer- 
enced in Table I; an inward recursion H\jfB is performed; 
and, the inverse articulated inertia D -1 is computed by 
simply taking the matrix inverse of D. Level 1 combines the 
results obtained in level 2 to produce the inverse Jacobian 
operator J _1 as (I-L*)D“ 1 HB\j/. 

Jacobian Operator J 


5 
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The modular assembly of independent low level operators 
comprising the Jacobian operator J is illustrated in FIG. 19. 
Referring to FIG. 19, level 7 consists of a coordinate 
transformation A fc 112. This is followed by level 6 consisting 
of an adjoint cross link operator \j/*(k+l,k). There is no cross 
joint operation. Thus, the operator t|r*(k+l,k) of level 6 is 
passed unchanged onto level 5 as an intrabody propagator. 
Level 4 performs an outward recursion by summing all 
intrabody propagators to generate a transition operator \| /*, 
which is assembled at level 3 with the adjoint pickoff 
operator B* and the adjoint joint axis operator H*. These 
operators are combined in level 2 as B*\|/*H* to generate the 
Jacobian operator J at level 1. 

Operational Space Inertia A 
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20 


25 


The seven level modular assembly comprising the opera- 
tional space inertia A is illustrated in FIG. 20 and is 
analogous to the inverse Jacobian J" 1 of FIG. 18. One 
difference is that the cross-link operation of Level 6 employs 30 
the operational space inertia A obtained at Level 1 during the 
previous iteration. The cross-joint operation of Level 6 is 
also different from that of FIG. 18 in that it employs an 
additional term, [I-G(k)H(k)]. The operator D 1 is obtained 
by taking the inverse of the operator D as in FIG. 18. Level 35 
2 performs only one computation: BV*H*D _1 H\|fB==A. 


Articulated Inertia D 

The seven level modular assembly comprising the articu- 40 
lated inertia D is illustrated in FIG. 21 and is analogous to 
the operational space inertia A of FIG. 20. Level 7 consists 
of a projection of the Kalman filter updated version of P(k) 
(whose definition is referenced in Table I) by the coordinate 
transformation operator A*, Level 6 then projects the version 
of P(k) generated in Level 7 in accordance with the cross- 45 
joint operator [I-G(k)H(k)]. Level 6 also projects the spatial 
inertia M(k) in accordance with the cross-link operation 
<j)(k,k-T). Combining the cross-link and cross-joint opera- 
tions of Level 6 produces the intrabody propagation of Level 
5, upon which an inward recursion is performed in Level 4. 50 
The resulting operator at Level 3 is projected upon H in 
Level 2 to produce the articulated inertia D at Level 1. The 
articulated inertia operator D is also described at line 4, 8th 
page of publication 1. The component of D for a given (kth) 
iteration, namely D(k), is defined by equation 38 on page 55 
630 of publication 3. 
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of levels 7 through 4 of FIG. 18, it being understood that this 
compilation is also present in FIG. 22. \| /* is obtained by 
taking the complex conjugate of \| / as in FIG. 18. Level 2 
produces the squeeze operator A*(C)=<j>*C*, the operational 
space inertia A of FIG. 20, a Kalman smoothing operation 
and a filtering operation (I-L). D" -1 is obtained by 
taking the matrix inverse of D as in FIG. 18. The operator 
L* is obtained at level 2 from H*, G* and \|i in accordance 
with the definition of L referred to in Table I as in FIG. 18. 
The operation of Level 1 is self-explanatory and employs the 
operational space inertia of FIG. 20 as one building block. 

Forward Kinematics X 

The seven level modular building block assembly of low 
level operators comprising the forward kinematics operator 
X is illustrated in FIG. 23. This structure is closely analo- 
gous to the structure of the Jacobian operator J illustrated in 
FIG. 19, the major difference being that coordinate trans- 
formations A l and Aj are employed in lieu of the propaga- 
tion operator $ in Level 6. The cross-link transformation A L 
performs a transformation from the frame of reference of the 
previous (k+lst) link to the frame of reference of the next 
(kth) link, and is a straightforward function of the robot 
configuration. The cross-joint transformation A j performs a 
transformation from one side of the current (kth) joint to the 
other side of the current joint and is also a straightforward 
function of the robot configuration. These two transforma- 
tions are combined in Level 5 in a combined transformation. 
The combined transformations thus adduced at Level 5 
during all previous iterations are multiplied together in the 
outward recursion of Level 4 to form the operator <j> at Level 
3. This latter operator is combined with the pickoff operator 
B at Level 2 to form the forward kinematics operator X at 
Level 1. 


Move Projection Operator F 

Referring to FIG. 24, the individual operations at each one 
of the seven levels comprising the compilation of the move 
projection operator F m have been described above with 
reference to respective ones of FIG.’S 18-23. 

Other high level operators may be formed to enhance the 
versatility of the high level operator language of the inven- 
tion. FIG. 25 illustrates a table of high level operators which 
may be formed using the seven level architecture discussed 
herein. In FIG. 25, a high level operator name with a 
numeral 1 indicates that the high level operator is useful in 
designing robots which perform no work on a task object 
and a high level operator name with a numeral 2 indicates 
that the high level operator is useful in designing robots 
which perform work (such as squeezing or pushing) on a 
task object. A potentially unlimited number of other high 
level operators may be formulated in the future to improve 
the efficiency and versatility of the high level operator robot 
language of the invention. 

LINEARIZED RECURSIVE HIGH LEVEL 
OPERATORS 


Squeeze Force Projection F^ 

The seven level modular assembly comprising the 
squeeze force projection operator is illustrated in FIG. 22. 
Levels 7 through 3 thereof are highly analogous to those of 
FIG. 19, except that the reference is always to the task object 
contact point operator C. As a result, the inward recursion of 
Level 4 is carried out at points on or in the task object. Level 
3 requires a contact point operator C to generate the squeeze 
operator A(C) of Level 2. Level 3 also requires the adjoint 
Kalman transition operator tjf* whose compilation consists 


One additional high level operator that has been recently 
introduced is illustrated in FIG. 26. The high level operator 
of FIG. 26 is a linearized dynamic operator which permits 
the user to specify a linearized dynamic robot model. 
Linearized robot dynamic models are particularly useful, for 
example, in controlling robot motion with respect to some 
pre-determined trajectory. In this application, control com- 
mands are computed with respect to small deviations of the 
actual robot motions from the pre-determined trajectory. A 
conventional variational technique is followed in linearized 
robot dynamics models which employs the partial derivative 


60 


65 
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of a given operator with respect to its parameters in order to 
compute the variation from a nominal state or trajectory. The 
implementation of linearized robot dynamics models using 
the spatial operator algebra of the present invention is 
disclosed by A Jain and G. Rodriguez, “Recursive Linear- 
ization of Manipulator Dynamics Models,” Proceedings of 
the IEEE Conference on Systems , Man and Cybernetics , 
Nov. 4, 1990. In the following description, the 8 symbol 
preceding an operator denotes the partial derivative of that 
operator with respect to each parameter (e.g., 0, 0) of the 
operator. It is a discovery of the invention that, in general, 
such partial derivatives of high level operators of the type 
referred to in the table of FIG. 25 are closed form expres- 
sions and are therefore susceptible of straightforward com- 
putation and use. 

It is a further discovery of the invention that high level 
operators of invention which are required to implement 
linearized robot dynamic models are easily constructed by 
simply copying the bottom four levels (i.e., Levels 7, 6, 5, 
4) of operators previously described herein, for example. 
Thus, only the top levels (i.e., Levels 3, 2, 1) need be 
customized or generated, greatly simplifying the task of 
generating such an operator. The operator of FIG. 26 is 8L, 
which is the partial derivative of the operator L (whose 
definition is referred to in Table I) with respect to 0, and is 
useful in implementing the linearized robot dynamics 
described above. FIG. 26 indicates that SL is a seven level 
modular assembly of low level operator building blocks, as 
is the case with all other high level operators described 
previously herein with respect to FIG.’S 18-23. The bottom 
four levels (i.e., levels 7, 6, 5 and 4) of the 8L operator of 
FIG. 26 are virtually identical (with the exception of a 
complex conjugate) to the bottom four levels of the inverse 
Jacobian operator J -1 previously described herein with ref- 
erence to FIG. 18 and therefore may be copied directly 
therefrom. 

Level 3 of the 5L operator of FIG. 26 provides the low 
level operators G, H and the partial derivative of H, namely 
8H. The inward recursion of Level 4 provides the Kalman 
transition operator \|f for Level 3. A partial derivative of \\f 
produces 5\|/ at Level 3. Level 2 combines 5H, G and \| / in 
a first operation and H, 5\y and G in a second parallel 
operation. Level 1 combines the results of the two operations 
to produce the linearized dynamic operator 8L. The appli- 
cation of this type of operator to linearized robot dynamics 
is described in A. Jain and G. Rodriguez, “Recursive Lin- 
earization of Manipulator Dynamics Models,” Proceedings 
of the IEEE Conference on Systems, Man and Cybernetics, 
Nov. 4, 1990. 

Wherefore, having thus described our invention, what is 
claimed is: 

1. A system for controlling a multi-link, multijoint robot 
comprising: 

a plurality of joint servos coupled to said robot; 

a plurality of joint sensors coupled to said plurality of 
joint servos; 

a processor coupled to said robot and responsive to said 
plurality of joint sensors for controlling said plurality of 
joint servos and for executing arithmetic instructions; 

first compiler means coupled to said processor for trans- 
lating each one of a selected sequence of plural low 
level spatial operators to an equivalent corresponding 
sequence of arithmetic instructions and transmitting 
said sequence of arithmetic instructions to said proces- 
sor, said set of said low level operators comprising: 
a first type of operator for performing a coordinate 
transformation to a frame of reference of a current 
robot link, 

a second type of operator for propagating vector quan- 
tities across at least one of said current robot link and 
a corresponding robot joint, 
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a third type of operator comprising a combination of 
operators of said second type, 
a fourth type of operator for performing a recursion of 
plural operators of said third type across plural robot 

5 links of a robot member, 

a fifth type of operator including operators produced 
from operators of said fourth type, and a sixth type 
of operator for combining operators of said fifth 
type; and 

10 second compiler means coupled to said processor for 
translating each one of a set of plural high level 
operators to an equivalent corresponding sequence of at 
least some of said low level operators and transmitting 
said sequence of low level operators to said first 
compiler means as said selected sequence, said 

15 sequence of low level operators comprising: 

plural iterations associated with plural discrete points in 
time, each iteration comprising: 
a first, second, third, fourth, fifth and sixth levels 
comprising, respectively, 

20 low level operators of said first, second, third, 

fourth, fifth and sixth types, whereby each high 
level operator comprises modular assembly of 
ones of said low level operators. 

2. The system of claim 1 further comprising: 

25 robot architecture memory means for storing a plurality of 
high level program statements in an order which 
includes said set of high level operators whereby to 
define a particular robot architecture, and wherein said 
memory means transmits said statements to said second 

30 compiler means in said order. 

3. The system of claim 1 wherein said fifth type of 
operator comprises a Kalman transition operator and said 
sixth type of operator comprises an operator which performs 
a Kalman filter operation in combining said operators of said 

35 fifth type. 

4. The system of claim 3 wherein said Kalman filter 
generates a Kalman gain for each one of said multi-links, 
and wherein said second type of operator which propagates 
vector quantities across a robot joint comprises said Kalman 
gain of said corresponding robot joint. 

40 5. The system of claim 3 wherein said fifth type of 

operator further comprises a robot joint axis projection 
operator and a pickoff projection operator. 

6. The system of claim 5 wherein the sequence of low 
level operators of one of said high level operators further 

45 comprises another one of said high level operators. 

7. The system of claim 5 wherein said recursion com- 
prises one of an inward recursion beginning at a tip joint and 
concluding at a base joint of said robot and an outward 
recursion beginning at said base joint and concluding at said 

50 tip joint. 

8. The system of claim 5 wherein one of said high level 
operators corresponds to a linearized robot dynamic model 
and at least one of the fifth type of operators thereof 
comprise partial derivatives of others of said fifth type of 

55 operators. 

9. The system of claim 5 further comprising designer 
input means, external of said robot and connected to said 
second compiler means, for naming a high level operator 
and for selecting plural ones of said low level operators and 
specifying connections among the selected plural low level 

60 operators corresponding to an equivalent sequence of said 
low level operators to which said second compiler means 
translates the high level operator named by said designer 
input means. 

10. The system of claim 5 wherein said first and second 

65 compiler means reside within said robot. 

11. A system for controlling a multi-link, multijoint robot 
comprising: 
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a plurality of joint servos coupled to said robot; 
a plurality of joint sensors coupled to said plurality of 
joint servos; 

a processor coupled to said robot and responsive to said 
plurality of joint sensors for controlling said plurality of 5 
joint servos and for executing arithmetic instructions; 
first compiler means coupled to said processor for trans- 
lating each one of a selected sequence of plural low 
level spatial operators to a sequence of arithmetic 
instructions which corresponds to each one of said 10 
selected sequence of plural low level spatial operators 
and transmitting said sequence of arithmetic instruc- 
tions to said processor, said set of said low level 
operators comprising: 

a first type of operator for performing a coordinate 15 
transformation to a frame of reference of a current 
robot link, 

a second type of operator for propagating vector quan- 
tities across at least one of said current robot link and 
a corresponding robot joint, 20 

a third type of operator comprising a combination of 
operators of said second type, 
a fourth type of operator for performing a recursion of 
plural operators of said third type across plural robot 
links of a robot member, 

a fifth type of operator including operators produced 25 
from operators of said fourth type, and a sixth type 
of operator for combining operators of said fifth 
type; 

second compiler means coupled to said processor for 
translating each one of a set of plural high level 30 
operators to a sequence of at least some of said low 
level operators which correspond to said high level 
operators and transmitting said sequence of low level 
operators to said first compiler means as said selected 
sequence, said sequence of low level operators com- 35 
prising: 

plural iterations associated with plural discrete points in 
time, each iteration comprising: 
a first, second, third, fourth, fifth and sixth levels 
comprising, respectively, 40 

low level operators of said first, second, third, 
fourth, fifth and sixth types, whereby each high 
level operator comprises a modular assembly 
of ones of said low level operators; 
wherein said fifth type of operator comprises a Kalman 45 
transition operator, a robot joint axis projection opera- 
tor, and a pickoff projection operator 
wherein said sixth type of operator comprises an operator 
which performs a Kalman filter operation in combining 
said operators of said fifth type; 50 

wherein the sequence of low level operators of one of said 
high level operators further comprises another one of 
said high level operators; and 
user input means external of said robot for entering said 
high level program statements into said robot architec- 55 
ture memory means. 

12. A method for controlling a multi-link, multijoint robot 
with a processor controlling a plurality of joint servos and 
responsive to a plurality of joint sensors for executing 
arithmetic instructions, said method comprising: 60 

compiling by said processor each one of a selected 
sequence of plural low level spatial operators to an 
equivalent corresponding sequence of arithmetic 
instructions and transmitting said sequence of arith- 
metic instructions to processor, said set of said low 
level operators comprising: 
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a first type of operator for performing a coordinate 
transformation to a frame of reference of a current 
robot link, 

a second type of operator for propagating vector quan- 
tities across at least one of said current robot link and 
a corresponding robot joint, 
a third type of operator comprising a combination of 
operators of said second type, 
a fourth type of operator for performing a recursion of 
plural operators of said third type across plural robot 
links of a robot member, 

a fifth type of operator including operators produced 
from operators of said fourth type, and 
a sixth type of operator for combining operators of said 
fifth type; 

translating each one of a set of plural high level operators 
to a corresponding sequence of at least some of said 
low level operators and transmitting said sequence of 
low level operators as said selected sequence for said 
compiling step, said sequence of low level operators 
comprising: 

plural iterations associated with plural discrete points in 
time, each iteration comprising: 
a first, second, third, fourth, fifth and sixth levels 
comprising, respectively, 
low level operators of said first, second, third, 
fourth, fifth and sixth types, whereby each high 
level operator comprises a modular assembly 
of ones of said low level operators. 

13. The method of claim 12 further comprising: 

storing a plurality of high level program statements in an 

order which includes selected ones of said high level 
operators whereby to define a particular robot archi- 
tecture, and transmitting said plurality of high level 
program statements as said set of plural high level 
operators for said translating step. 

14. The method of claim 12 wherein said fifth type of 
operator comprises a Kalman transition operator and said 
sixth type of operator comprises an operator which performs 
a Kalman filter operation in combining said operators of said 
fifth type. 

15. The method of claim 14 wherein said Kalman filter 
generates a Kalman gain for each one of said multi-links, 
and wherein said second type of operator which propagates 
vector quantities across a robot joint comprises said Kalman 
gain of said corresponding robot joint. 

16. The method of claim 14 wherein said fifth type of 
operator further comprises a robot joint axis projection 
operator and a pickoff projection operator. 

17. The method of claim 16 further comprising naming a 
high level operator, selecting plural ones of said low level 
operators and specifying connections among the selected 
plural low level operators corresponding to an equivalent 
sequence of said low level operators to which said translat- 
ing step translates the named high level operator. 

18. The method of claim 12 wherein the sequence of low 
level operators of one of said high level operators further 
comprises another one of said high level operators at one of 
said first, second and third levels. 

19. The method of claim 12 wherein said recursion 
comprises one of an inward recursion beginning at a tip joint 
and concluding at a base joint of said robot and an outward 
recursion beginning at said base joint and concluding at said 
tip joint. 

20. The method of claim 12 wherein one of said high level 
operators corresponds to a linearized robot dynamic model 
and at least one of the fifth type of operators thereof 
comprise partial derivatives of others of said fifth type of 
operators. 

* * * * * 



